#!/usr/bin/env python

from dynamic_reconfigure.parameter_generator_catkin import *
PACKAGE = "spherical_grasps_server"

gen = ParameterGenerator()

gen.add("grasp_postures_frame_id", str_t, 0,
        "pre_grasp_posture and grasp_posture frame_id, usually the tool (previous to the gripper)",
        "arm_tool_link")

gen.add("gripper_joint_names", str_t, 0, "gripper joint names",
        "gripper_left_finger_joint gripper_right_finger_joint")
gen.add("gripper_pre_grasp_positions", str_t, 0,
        "gripper pre_grasp joint positions (same order than joint_names)",
        "0.05 0.05")
gen.add("gripper_grasp_positions", str_t, 0,
        "gripper pre_grasp joint positions (same order than joint_names)",
        "0.01 0.01")

gen.add("time_pre_grasp_posture", double_t, 0,
        "Time for the pre_grasp gripper posture to be achieved",
        2.0, 0.0, 10.0)
gen.add("time_grasp_posture", double_t, 0,
        "Time for the grasp gripper posture to be achieved (will be added to pre_grasp_posture)",  # NOQA
        2.0, 0.0, 10.0)
gen.add("time_grasp_posture_final", double_t, 0,
        "Time for the grasp gripper final posture to be achieved (will be added to time_grasp_posture)",  # NOQA
        2.0, 0.0, 10.0)

gen.add("grasp_pose_frame_id", str_t, 0,
        "grasp pose frame id is the frame where the object pose is expressed. Pose must be in the frame of MoveIt's base frame, base_footprint usually",
        "base_footprint")

gen.add("grasp_desired_distance", double_t, 0,
        "grasp desired distance for pre grasp, post grasp and place",
        0.15, 0.0, 1.0)
gen.add("grasp_min_distance", double_t, 0,
        "grasp min distance for pre grasp, post grasp and place",
        0.0, 0.0, 1.0)



# gen.add("default_im_x", double_t, 0,
#         "Initial IM x coord in reference to grasp_pose_frame_id",
#         0.5, 0.0, 2.0)
# gen.add("default_im_y", double_t, 0,
#         "Initial IM y coord in reference to grasp_pose_frame_id",
#         0.0, 0.0, 2.0)
# gen.add("default_im_z", double_t, 0,
#         "Initial IM z coord in reference to grasp_pose_frame_id",
#         0.8, 0.0, 2.0)

# gen.add("default_im_roll", double_t, 0,
#         "Initial IM roll in degrees in reference to grasp_pose_frame_id",
#         0.0, -180.0, 180.0)
# gen.add("default_im_pitch", double_t, 0,
#         "Initial IM pitch in degrees in reference to grasp_pose_frame_id",
#         0.0, -180.0, 180.0)
# gen.add("default_im_yaw", double_t, 0,
#         "Initial IM yaw in degrees in reference to grasp_pose_frame_id",
#         0.0, -180.0, 180.0)

# gen.add("pre_grasp_approach_frame_id", str_t, 0,
#         "pre grasp approach frame id for the direction",
#         "gripper_grasping_frame")
gen.add("pre_grasp_direction_x", double_t, 0,
        "Pre grasp approach x direction vector",
        1.0, -1.0, 1.0)
gen.add("pre_grasp_direction_y", double_t, 0,
        "Pre grasp approach y direction vector",
        0.0, -1.0, 1.0)
gen.add("pre_grasp_direction_z", double_t, 0,
        "Pre grasp approach z direction vector",
        0.0, -1.0, 1.0)
# gen.add("pre_grasp_desired_distance", double_t, 0,
#         "Pre grasp desired distance",
#         0.15, 0.0, 1.0)
# gen.add("pre_grasp_min_distance", double_t, 0,
#         "Pre grasp min distance",
#         0.0, 0.0, 1.0)

# gen.add("post_grasp_retreat_frame_id", str_t, 0,
#         "post grasp retreat frame id for the direction",
#         "gripper_grasping_frame")
gen.add("post_grasp_direction_x", double_t, 0,
        "Post grasp retreat x direction vector",
        -1.0, -1.0, 1.0)
gen.add("post_grasp_direction_y", double_t, 0,
        "Post grasp retreat y direction vector",
        0.0, -1.0, 1.0)
gen.add("post_grasp_direction_z", double_t, 0,
        "Post grasp retreat z direction vector",
        0.0, -1.0, 1.0)
# gen.add("post_grasp_desired_distance", double_t, 0,
#         "Post grasp desired distance",
#         0.0, 0.0, 1.0)
# gen.add("post_grasp_min_distance", double_t, 0,
#         "Post grasp min distance",
#         0.0, 0.0, 1.0)

# In case I wanna also test in the future
# gen.add("post_place_retreat_frame_id", str_t, 0,
#         "post_place_retreat frame id for the direction",
#         "gripper_grasping_frame")
# gen.add("post_place_retreat_direction_x", double_t, 0,
#         "Post grasp retreat x direction vector",
#         -1.0, -1.0, 1.0)
# gen.add("post_place_retreat_direction_y", double_t, 0,
#         "post_place_retreat y direction vector",
#         0.0, -1.0, 1.0)
# gen.add("post_place_retreat_direction_z", double_t, 0,
#         "post_place_retreat z direction vector",
#         0.0, -1.0, 1.0)
# gen.add("post_place_retreat_desired_distance", double_t, 0,
#         "post_place_retreat desired distance",
#         0.0, 0.0, 1.0)
# gen.add("post_place_retreat_min_distance", double_t, 0,
#         "Post grasp min distance",
#         0.0, 0.0, 1.0)


gen.add("grasp_quality", double_t, 0,
        "grasp quality to set to the grasp", 0.1, 0.0, 1.0)


gen.add("max_contact_force", double_t, 0,
        "max_contact_force", 0.0, -1.0, 1.0)

gen.add("allowed_touch_objects", str_t, 0,
        "allowed_touch_objects",
        "")

# Fix rotation from the tool_link to the grasping frame
# The pick and place pipeline actually does not use the rotation of the
# grasp poses frame but the parent of them, so we need to manually
# give the transformation
gen.add("fix_tool_frame_to_grasping_frame_roll", double_t, 0,
        "Fix the transformation of the tool_link frame to the grasping_frame roll (degrees)",
        0.0, -360.0, 360)
gen.add("fix_tool_frame_to_grasping_frame_pitch", double_t, 0,
        "Fix the transformation of the tool_link frame to the grasping_frame pitch (degrees)",
        0.0, -360.0, 360)
gen.add("fix_tool_frame_to_grasping_frame_yaw", double_t, 0,
        "Fix the transformation of the tool_link frame to the grasping_frame yaw (degrees)",
        0.0, -360.0, 360)

gen.add("step_degrees_yaw", int_t, 0,
        "Degrees step over yaw on generating the grasp poses",
        10, 1, 360)
gen.add("step_degrees_pitch", int_t, 0,
        "Degrees step over pitch on generating the grasp poses",
        10, 1, 360)

gen.add("min_degrees_yaw", int_t, 0,
        "Starting degrees over yaw on generating grasp poses",
        0, 0, 360)
gen.add("max_degrees_yaw", int_t, 0,
        "Stopping degrees over yaw on generating grasp poses",
        360, 0, 360)
gen.add("min_degrees_pitch", int_t, 0,
        "Starting degrees over pitch on generating grasp poses",
        0, 0, 360)
gen.add("max_degrees_pitch", int_t, 0,
        "Stopping degrees over pitch on generating grasp poses",
        360, 0, 360)

exit(gen.generate(PACKAGE, PACKAGE, "SphericalGrasp"))
